#include "io_obsta_avoid.hpp"

using namespace std;
using namespace tinyxml2;

namespace AgvObstaAvoid {

IoObstaAvoid::IoObstaAvoid(rclcpp::Node::SharedPtr node)
: node_(node), lidar_error_(false)
{}

bool IoObstaAvoid::IOReadXml(XMLElement* element) {
    bEquipment equip_io;
    RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "init io equipment xml param");
    try
    {
        equip_io.name = stoi(element->Attribute("name"));
        equip_io.docount = stoi(element->Attribute("docount"));
        equip_io.pose = stoi(element->Attribute("pose"));

        XMLElement* area_node = element->FirstChildElement();
        if (!area_node)
        {
            RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "io equipment:%d area node is nullptr, pos:%d", equip_io.name, equip_io.pose);
        } else 
        {
            while (area_node)
            {
                bArea area_tem;
                area_tem.name = stoi(area_node->Attribute("name"));

                for (size_t i = 0; i < equip_io.docount; i++)
                {
                    string ctrl = "ctrl" + to_string(i + 1);
                    area_tem.interface.push_back(stoi(area_node->Attribute(ctrl.data())));
                }
                
                equip_io.arealist.push_back(area_tem);
                area_node = area_node->NextSiblingElement();
            }
        }
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
        RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "io equipment init xml param faild");
        return false;
    }

    RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "io equipment init xml param successed");
    io_equip_list_.push_back(equip_io);

    RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "equip name: %d docount: %d", equip_io.name, equip_io.docount);
    if (!equip_io.arealist.empty())
    {
        for (int i = 0; i < equip_io.arealist.size(); i++)
        {
            ShowEquipParam(&equip_io.arealist.at(i));
        }
    }
    
    return true;
}

void IoObstaAvoid::ShowEquipParam(bArea* area) {
    RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "area name: %d", area->name);
    for (int i = 0; i < area->interface.size(); i++)
    {
        RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "ctrl%d:%d", i+1, area->interface[i]);
    }
    return;
}

void IoObstaAvoid::readInputCallBack(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg)
{
    refresh_avoidState(msg);
}

void IoObstaAvoid::autoStatusCallBack(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg)
{
    int from_node = msg->fromnode;
    int to_node = msg->tonode;
    
    if (!msg->task_node_id.empty()){
        int task_id = stoi(msg->task_node_id);
        if (task_id != to_node) {
            fork_avoid_.task_id = true;
        } else {
            fork_avoid_.task_id = false;
        }

    } else {
        fork_avoid_.task_id = false;
    }
    
    if (msg->wheelmovedir == 1)
        fork_avoid_.vehicle_back = true;
    else 
        fork_avoid_.vehicle_back = false;

    if (msg->task_action_type.compare("pick") == 0)
        fork_avoid_.action_type = true;
    else 
        fork_avoid_.action_type = false;
}

void IoObstaAvoid::jointStateCallBack(const sensor_msgs::msg::JointState::SharedPtr msg)
{
    if (fork_avoid_.vehicle_type) {
        for (int i = 0; i < msg->name.size(); i++) // find forkLift_joint
        {
            if (msg->name[i].find("forkLift_joint") != string::npos) {
                double high = msg->position[i];
                if (high > 0.34) {
                    fork_avoid_.lift_high = true;
                } else {
                    fork_avoid_.lift_high = false;
                }
            }
        }
    }
}

void IoObstaAvoid::refresh_avoidState(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg)
{
    lock_guard<mutex> lock(io_State_mutex_);
    bool ossd_flag = false;
    for (size_t j = 0; j < msg->array.size(); j++)
    {
        auto io_avoid_msg = msg->array[j];

        if (io_avoid_msg.key.find("front") != std::string::npos)
        {
            // 前避障输入
            if (io_avoid_msg.key == "frontStop")
                front_input_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "frontSlow")
                front_input_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "frontWarning")
                front_input_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "frontError")
                front_input_.input_list[3].second = io_avoid_msg.value;
        }
        
        if (io_avoid_msg.key.find("rear") != std::string::npos)
        {
            // 后避障输入
            if (io_avoid_msg.key == "rearStop")
                rear_input_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rearSlow")
                rear_input_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rearWarning")
                rear_input_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rearError")
                rear_input_.input_list[3].second = io_avoid_msg.value;
        }

        if (io_avoid_msg.key.find("left") != std::string::npos)
        {
            // 左避障输入
            if (io_avoid_msg.key == "leftStop")
                left_input_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftSlow")
                left_input_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftWarning")
                left_input_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftError")
                left_input_.input_list[3].second = io_avoid_msg.value;
            
            // 左前避障输入
            if (io_avoid_msg.key == "leftFrontStop")
                left_front_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftFrontSlow")
                left_front_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftFrontWarning")
                left_front_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftFrontError")
                left_front_.input_list[3].second = io_avoid_msg.value;
        
            // 左后避障输入
            if (io_avoid_msg.key == "leftRearStop")
                left_rear_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftRearSlow")
                left_rear_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftRearWarning")
                left_rear_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftRearError")
                left_rear_.input_list[3].second = io_avoid_msg.value;
        }
        
        if (io_avoid_msg.key.find("right") != std::string::npos)
        {
            // 右避障输入
            if (io_avoid_msg.key == "rightStop")
                right_input_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightSlow")
                right_input_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightWarning")
                right_input_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightError")
                right_input_.input_list[3].second = io_avoid_msg.value;

            // 右前避障输入
            if (io_avoid_msg.key == "rightFrontStop")
                right_front_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightFrontSlow")
                right_front_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightFrontWarning")
                right_front_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightFrontError")
                right_front_.input_list[3].second = io_avoid_msg.value;

            // 右后避障输入
            if (io_avoid_msg.key == "rightRearStop")
                right_rear_.input_list[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightRearSlow")
                right_rear_.input_list[1].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightRearWarning")
                right_rear_.input_list[2].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightRearError")
                right_rear_.input_list[3].second = io_avoid_msg.value;
        }

        if (io_avoid_msg.key.find("Tip") != std::string::npos)
        {
            if (io_avoid_msg.key.compare("leftForkTipSensor") == 0)
                read_input_data_.leftTipSensor = (io_avoid_msg.value == 1);

            if (io_avoid_msg.key.compare("rightForkTipSensor") == 0)
                read_input_data_.rightTipSensor = (io_avoid_msg.value == 1);

            if (io_avoid_msg.key.compare("leftForkTipTouchSensor") == 0)
                read_input_data_.leftTipTouchSensor = (io_avoid_msg.value == 1);

            if (io_avoid_msg.key.compare("rightForkTipTouchSensor") == 0)
                read_input_data_.rightTipTouchSensor = (io_avoid_msg.value == 1);

            fork_tipsensor_.input_list[0].second = read_input_data_.leftTipSensor || read_input_data_.rightTipSensor;
            fork_tipsensor_.input_list[1].second = false;
            fork_tipsensor_.input_list[2].second = false;
            fork_tipsensor_.input_list[3].second = false;
        }
        
        if (io_avoid_msg.key.find("LoadInplace") != std::string::npos || io_avoid_msg.key.find("loadStatus") != std::string::npos)
        {
            if (io_avoid_msg.key.compare("leftLoadInplace") == 0)
            {
                read_input_data_.leftLoadInplace = (io_avoid_msg.value == 1);
            }
            if (io_avoid_msg.key.compare("rightLoadInplace") == 0)
            {
                read_input_data_.rightLoadInplace = (io_avoid_msg.value == 1);
            }
            if (io_avoid_msg.key.compare("loadStatus") == 0)
            {
                read_input_data_.loadStatus = (io_avoid_msg.value == 1);
            }

            fork_loadinplace_.input_list[0].second = read_input_data_.leftLoadInplace || read_input_data_.rightLoadInplace;
            fork_loadinplace_.input_list[1].second = read_input_data_.loadStatus;
            fork_loadinplace_.input_list[2].second = false;
            fork_loadinplace_.input_list[3].second = false;
        }

        if (io_avoid_msg.key.find("Ossd") != std::string::npos)
        {
            ossd_flag = true;

            if (io_avoid_msg.key == "rearOssd1")
                rear_ossd_.ossd_input[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rearOssd2")
                rear_ossd_.ossd_input[1].second = io_avoid_msg.value;
            
            if (io_avoid_msg.key == "rightOssd1")
                right_ossd_.ossd_input[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "rightOssd2")
                right_ossd_.ossd_input[1].second = io_avoid_msg.value;

            if (io_avoid_msg.key == "leftOssd1")
                left_ossd_.ossd_input[0].second = io_avoid_msg.value;
            if (io_avoid_msg.key == "leftOssd2")
                left_ossd_.ossd_input[1].second = io_avoid_msg.value;
        }
    }

    if (ossd_flag)
    {
        rear_input_.input_list[0].second = (rear_ossd_.ossd_input[0].second && rear_ossd_.ossd_input[1].second);
        right_input_.input_list[0].second = (right_ossd_.ossd_input[0].second && right_ossd_.ossd_input[1].second);
        left_input_.input_list[0].second = (left_ossd_.ossd_input[0].second && left_ossd_.ossd_input[1].second);
    }

    if (left_input_.input_list[3].second || right_input_.input_list[3].second || rear_input_.input_list[3].second || 
        front_input_.input_list[3].second || left_front_.input_list[3].second || left_rear_.input_list[3].second ||
        right_front_.input_list[3].second || right_rear_.input_list[3].second) 
    {
        lidar_error_ = true;
    } else {
        lidar_error_ = false;
    }
}

void IoObstaAvoid::peripheralCallback(const agv_msgs::msg::PeriphStatus::SharedPtr load_msg)
{
    bool load = int(load_msg->loaded);
    if (load) {
        load_status_ = true; //负载
    } else {
        load_status_ = false; //空载
    }
}

std::vector<agv_msgs::msg::AvoidObsta> IoObstaAvoid::ScanCheck(std::vector<ScenesJudge::cEquip>& io_equip) 
{
    lock_guard<mutex> sc_mutex(io_State_mutex_);
    std::vector<agv_msgs::msg::AvoidObsta> avoid_msg;
    for (size_t i = 0; i < io_equip.size(); i++)
    {
        // 获取雷达安装位置
        int mount_position = -1;
        for (size_t j = 0; j < io_equip_list_.size(); j++)
        {
            if (io_equip[i].name == io_equip_list_[j].name) {
                mount_position = io_equip_list_[j].pose;
                break;
            }
        }
        agv_msgs::msg::AvoidObsta io_avoid_msg;
        io_avoid_msg.equipment = io_equip[i].name;
        io_avoid_msg.pose = mount_position;
        io_avoid_msg.area = io_equip[i].area;

        InputSignal signal_io = posInput(mount_position);
        // 针对叉车后避障雷达特殊逻辑
        if (fork_avoid_.vehicle_type && (mount_position == 1)) 
        {
            if (io_equip[i].stopenable && fork_avoid_.lift_high) {
                io_avoid_msg.stop = signal_io.input_list[0].second;
            } else {
                io_avoid_msg.stop = false;
            }

            if (io_equip[i].slowenable && fork_avoid_.lift_high) {
                io_avoid_msg.slow = signal_io.input_list[1].second;
            } else {
                io_avoid_msg.slow = false;
            }
            
            if (io_equip[i].warnenable && fork_avoid_.lift_high) {
                io_avoid_msg.warn = signal_io.input_list[2].second;
            } else {
                io_avoid_msg.warn = false;
            }
        } else if (fork_avoid_.vehicle_type && (mount_position == 8))    //叉尖避障
        {
            if (io_equip[i].stopenable && fork_avoid_.vehicle_back) {
                io_avoid_msg.stop = signal_io.input_list[0].second;
            } else {
                io_avoid_msg.stop = false;
            }

            if (io_equip[i].slowenable && fork_avoid_.vehicle_back) {
                io_avoid_msg.slow = signal_io.input_list[1].second;
            } else {
                io_avoid_msg.slow = false;
            }
            
            if (io_equip[i].warnenable && fork_avoid_.vehicle_back) {
                io_avoid_msg.warn = signal_io.input_list[2].second;
            } else {
                io_avoid_msg.warn = false;
            }
        } else if (fork_avoid_.vehicle_type && (mount_position == 9))    //载货光电避障
        {
            if (io_equip[i].stopenable && fork_avoid_.vehicle_back && fork_avoid_.action_type && fork_avoid_.task_id) {
                io_avoid_msg.stop = signal_io.input_list[0].second;
            } else {
                io_avoid_msg.stop = false;
            }

            if (io_equip[i].slowenable && fork_avoid_.vehicle_back && fork_avoid_.action_type && fork_avoid_.task_id) {
                io_avoid_msg.slow = signal_io.input_list[1].second;
            } else {
                io_avoid_msg.slow = false;
            }
            
            if (io_equip[i].warnenable && fork_avoid_.vehicle_back && fork_avoid_.action_type && fork_avoid_.task_id) {
                io_avoid_msg.warn = signal_io.input_list[2].second;
            } else {
                io_avoid_msg.warn = false;
            }
        }
        
        // 正常处理
        else 
        {
            if (io_equip[i].stopenable) {
                io_avoid_msg.stop = signal_io.input_list[0].second;
            } else {
                io_avoid_msg.stop = false;
            }

            if (io_equip[i].slowenable) {
                io_avoid_msg.slow = signal_io.input_list[1].second;
            } else {
                io_avoid_msg.slow = false;
            }
            
            if (io_equip[i].warnenable) {
                io_avoid_msg.warn = signal_io.input_list[2].second;
            } else {
                io_avoid_msg.warn = false;
            }
        }
        avoid_msg.push_back(io_avoid_msg);
    }
    return avoid_msg;
}

IoObstaAvoid::InputSignal IoObstaAvoid::posInput(int position)
{
    InputSignal tem_signal;

    switch (position)
    {
    case FRONT:
        tem_signal = front_input_;
        break;
    case REAR:
        tem_signal = rear_input_;
        break;
    case LEFT:
        tem_signal = left_input_;
        break;
    case RIGHT:
        tem_signal = right_input_;
        break;
    case LEFTFRONT:
        tem_signal = left_front_;
        break;
    case LEFTREAR:
        tem_signal = left_rear_;
        break;
    case RIGHTFRONT:
        tem_signal = right_front_;
        break;
    case ROGHTREAR:
        tem_signal = right_rear_;
        break;
    case TIPSENSOR:
        tem_signal = fork_tipsensor_;
        break;
    case LOADINPLACE:
        tem_signal = fork_loadinplace_;
        break;
    default:
        break;
    }
    
    return tem_signal;
}

vector<string> IoObstaAvoid::getZoneSwitch(int position)
{
    vector<std::string> tem_zone;
    
    switch (position)
    {
    case FRONT:
        tem_zone = frontzone_list_;
        break;
    
    case REAR:
        tem_zone = rearzone_list_;
        break;

    case LEFT:
        tem_zone = leftzone_list_;
        break;
    
    case RIGHT:
        tem_zone = rightzone_list_;
        break;
    
    case LEFTFRONT:
        tem_zone = leftFront_list_;
        break;

    case LEFTREAR:
        tem_zone = leftRear_list_;
        break;

    case RIGHTFRONT:
        tem_zone = rightFront_list_;
        break;
    
    case ROGHTREAR:
        tem_zone = rightRear_list_;
        break;
    
    default:
        break;
    }

    return tem_zone;
}

// IO设备切换区域
bool IoObstaAvoid::outputMap2IO(std::vector<ScenesJudge::cEquip>& states)
{
    if (io_equip_list_.empty())
    {
        return true;
    }
    
    bool ret = false;
    std::vector<agv_msgs::msg::PairTypeInt> outports;
    auto srv = std::make_shared<agv_srvs::srv::OutputsNew::Request>();
    RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "outputMap2Io");

    for (size_t i = 0; i < states.size(); i++)
    {
        int equip_index = states[i].name;   
        int area_index = states[i].area;
        if (area_index == 0)
        {
            continue;
        }
        
        for (size_t j = 0; j < io_equip_list_.size(); j++)
        {
            if (equip_index == io_equip_list_[j].name)
            {
                agv_msgs::msg::PairTypeInt singleIo;
                auto io_tem = io_equip_list_[j];
                int position = io_tem.pose;
                if (position < 0 || position > 9)
                {
                    RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "lidar position cross the border");
                    return false;
                }
                
                vector<string> zone_tem = getZoneSwitch(position);
                for (size_t k = 0; k < io_tem.docount; k++)
                {
                    if (ioExist(zone_tem[k]))
                    {
                        singleIo.key = zone_tem[k];
                        singleIo.value = io_tem.arealist[area_index-1].interface[k];
                        outports.push_back(singleIo);
                    }
                }
                break;
            }
        }
    }

    srv->array = outports;
    auto result = io_client_->async_send_request(srv);
    if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) == 
        rclcpp::FutureReturnCode::SUCCESS)
    {
        ret = result.get()->state;
        if (ret) {
            RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "io area change set success");
        } else {
            RCLCPP_ERROR(rclcpp::get_logger("IoObstaAvoid"), "io area change set failed");
        }
    }
    else
    {
        ret = false;
        RCLCPP_ERROR(rclcpp::get_logger("IoObstaAvoid"), "Failed to call service outputNew");
    }

    return ret;
}

bool IoObstaAvoid::FaultDetec()
{
    if (lidar_error_)
        return true;

    return false;
}

bool IoObstaAvoid::ioExist(std::string key)
{
    for (auto item : io_output_list_)
    {
        if (item == key) {
            return true;
        }
    }
    return false;
}

bool IoObstaAvoid::init()
{
    if (io_equip_list_.empty()){
        return false;
    }
    
    inputmap_sub_ = node_->create_subscription<agv_msgs::msg::ReadInPutsNew>(
        "/readInputNew", 2, std::bind(&IoObstaAvoid::readInputCallBack, this, std::placeholders::_1));
    
    // auto_status_sub_ = node_->create_subscription<agv_msgs::msg::AgvAutoStatus>(
    //     "/auto_run_status", 2, std::bind(&IoObstaAvoid::autoStatusCallBack, this, std::placeholders::_1));
    
    jointstate_sub_ = node_->create_subscription<sensor_msgs::msg::JointState>(
        "/base/joint_states", 2, std::bind(&IoObstaAvoid::jointStateCallBack, this, std::placeholders::_1));
    
    // peripheral_sub_ = node_->create_subscription<agv_msgs::msg::PeriphStatus>(
    //     "/periphStatus", 2, std::bind(&IoObstaAvoid::peripheralCallback, this, std::placeholders::_1));
    
    io_client_ = node_->create_client<agv_srvs::srv::OutputsNew>("/outputNew");
    
    const char* robotEnv = getenv("ROBOT");  
    fork_avoid_.vehicle_type = (robotEnv != nullptr && std::string(robotEnv).compare("fork") == 0);
    RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "fork_vehicle: %d", fork_avoid_.vehicle_type);
    
    // todo use shared_parameters
    // 获取所有切换避障区域的输出信号
    std::vector<std::string> param_names = node_->list_parameters({}, 0).names;
    for (auto param : param_names)
    {
        std::size_t pos_out = param.find("IO.output_device.");
        if(pos_out != std::string::npos) {
            std::string key = param.erase(pos_out, std::string("IO.output_device.").length());
            std::size_t zone_out = key.find("Switch");
            if (zone_out != std::string::npos)
            {
                io_output_list_.push_back(key);
            }
        }
    }

    for (size_t i = 0; i < io_output_list_.size(); i++)
    {
        RCLCPP_INFO(rclcpp::get_logger("IoObstaAvoid"), "io_output_list :%s", io_output_list_[i].c_str());
    }

    return true;
}

}   // namespace AgvObstaAvoid end